/**
 * @file
 * @author dousha 
 * @date 2021/5/15.
 */

#include "protocol.h"
#include "solver.h"
#include "usbd_cdc_if.h"
#include "gps.h"
#include "system.h"

static uint8_t state = 0, remainingBytes = 0, collectedBytes = 0;
static uint8_t buf[16] = { 0 }, out[16] = { 0 };

static uint16_t parse_u3dot1(const uint8_t *xs) {
	return ((xs[0] - '0') * 1000)
	+ ((xs[1] - '0') * 100)
	+ ((xs[2] - '0') * 10)
	+ ((xs[4] - '0'));
}

void protocol_process(uint8_t *data, size_t length) {
	size_t offset = 0;
	while (offset < length) {
		switch (state) {
			default:
				state = 0;
			case 0:
				if (data[offset] == '$') {
					state = 1;
				}
				break;
			case 1:
				if (data[offset] == 'X') {
					state = 2;
				} else if (data[offset] == 'M') {
					state = 200;
				} else {
					state = 0;
				}
				break;
			case 2:
				switch (data[offset]) {
					case 'T':
						state = 3;
						remainingBytes = 5; // u3.1 format
						collectedBytes = 0;
						break;
					case 'G':
						state = 30;
						remainingBytes = 11; // latitude, 4.4,x
						collectedBytes = 0;
						break;
					case 'g':
						state = 40;
						remainingBytes = 8; // latitude 26d
						collectedBytes = 0;
						break;
					case 'Y':
						state = 5;
						remainingBytes = 5; // u3.1 format
						collectedBytes = 0;
						break;
					case 'X':
						state = 0;
						system_set_mode(0);
						break;
					case 'A':
						state = 0;
						system_set_mode(1);
						break;
					case 'S': // set current gps as remote
						state = 0;
						solver_set_current_as_remote();
						break;
					case '?':
						state = 0;
						system_set_mode(2);
						break;
					case 'N':
						state = 0;
						solver_mark_current_yaw_as_zero();
						break;
					default:
						state = 0;
						break;
				}
				break;
			case 3:
				if (collectedBytes < remainingBytes) {
					buf[collectedBytes++] = data[offset];
				} else {
					// pitch parsed
					CDC_Transmit_FS(buf, 5);
					solver_set_target_pitch(parse_u3dot1(buf));
					state = 4;
					remainingBytes = 5;
					collectedBytes = 0;
				}
				break;
			case 4:
				if (collectedBytes < remainingBytes) {
					buf[collectedBytes++] = data[offset];
				} else {
					// yaw parsed
					CDC_Transmit_FS(buf, 5);
					solver_set_target_yaw(parse_u3dot1(buf));
					state = 0;
					remainingBytes = 0;
					collectedBytes = 0;
				}
				break;
			case 5:
				if (collectedBytes < remainingBytes) {
					buf[collectedBytes++] = data[offset];
				} else {
					// given yaw
					CDC_Transmit_FS(buf, 5);
					solver_set_self_yaw(parse_u3dot1(buf));
					state = 0;
					remainingBytes = 0;
					collectedBytes = 0;
				}
				break;
			case 30:
				if (collectedBytes < remainingBytes) {
					buf[collectedBytes++] = data[offset];
				} else {
					// lat parsed
					CDC_Transmit_FS(buf, 11);
					solver_set_remote_lat(parse_latitude(buf));
					state = 31;
					remainingBytes = 12; // lng, 5.4,x
					collectedBytes = 0;
				}
				break;
			case 31:
				if (collectedBytes < remainingBytes) {
					buf[collectedBytes++] = data[offset];
				} else {
					// lat parsed
					CDC_Transmit_FS(buf, 12);
					solver_set_remote_lng(parse_longitude(buf));
					state = 32;
					remainingBytes = 5; // alt, 3.1
					collectedBytes = 0;
				}
				break;
			case 32:
				if (collectedBytes < remainingBytes) {
					buf[collectedBytes++] = data[offset];
				} else {
					// alt parsed
					CDC_Transmit_FS(buf, 5);
					solver_set_remote_alt(parse_u3dot1(buf));
					state = 0;
					remainingBytes = 0;
					collectedBytes = 0;
				}
				break;
			case 40:
				if (collectedBytes < remainingBytes) {
					buf[collectedBytes++] = data[offset];
				} else {
					// lat parsed
					solver_set_remote_lat(parse_direct_latitude(buf));
					state = 41;
					remainingBytes = 9; // lng, 36d
					collectedBytes = 0;
				}
				break;
			case 41:
				if (collectedBytes < remainingBytes) {
					buf[collectedBytes++] = data[offset];
				} else {
					// lat parsed
					solver_set_remote_lng(parse_direct_longitude(buf));
					state = 32;
					remainingBytes = 5; // alt, 3.1
					collectedBytes = 0;
				}
				break;
		}
		++offset;
	}
	if (remainingBytes != 0) {
		out[0] = '_';
		out[1] = '0' + collectedBytes;
		out[2] = '/';
		out[3] = '0' + remainingBytes;
		CDC_Transmit_FS(out, 4);
	} else {
		out[0] = '0' + state;
		CDC_Transmit_FS(out, 1);
	}
}
